#ifndef SRC_POLYFIT_H
#define SRC_POLYFIT_H
#include "eigen/Eigen/Dense"

namespace polyFit_NS {
// 根据多项式系数，计算多项式在x处的值.
inline double polyEval(const Eigen::VectorXd &coeffs, double x) {
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++) {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

// 用多项式拟合路径点，改编自:https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
inline Eigen::VectorXd polyFit(const Eigen::VectorXd &x_values, const Eigen::VectorXd &y_values,
                               int order) {
//    assert(x_values.size() == y_values.size());
//    assert(order >= 1 && order <= x_values.size() - 1);
    Eigen::MatrixXd A(x_values.size(), order + 1);

    for (int i = 0; i < x_values.size(); i++) {
        A(i, 0) = 1.0;
    }
    for (int j = 0; j < x_values.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * x_values(j);
        }
    }

    auto Q = A.householderQr();
    auto result = Q.solve(y_values);
    return result;
}
}   //namespace polyFit_NS
#endif //SRC_POLYFIT_H
